/*Includes ----------------------------------------------------------------------------------------*/
#include "stm32f10x.h"
#include <stdio.h>
#include <string.h>
#include "led.h"
#include "oled.h"
#include "coder.h"
#include "jy901.h"
#include "sger.h"
#include "uart2.h"
#include "logic.h"
#include "motor.h"
#include "tracing.h"
#include "readiolevel.h"

int projects = 0;
extern u8 *p;
extern u8 flag;
extern u8 sp;
struct SAngle 	stcAngle;

int ccc =0;

float aaaa = 0.0;

u8 jilu[3] = {0};

//CopeSerialData为串口2中断调用函数，串口每收到一个数据，调用一次这个函数。
void CopeSerial2Data(unsigned char ucData)
{
	static unsigned char ucRxBuffer[250];
	static unsigned char ucRxCnt = 0;	
	
	ucRxBuffer[ucRxCnt++]=ucData;	//将收到的数据存入缓冲区中
	if (ucRxBuffer[0]!=0x55) //数据头不对，则重新开始寻找0x55数据头
	{
		ucRxCnt=0;
		return;
	}
	if (ucRxCnt<11) {return;}//数据不满11个，则返回
	else
	{
		switch(ucRxBuffer[1])//判断数据是哪种数据，然后将其拷贝到对应的结构体中，有些数据包需要通过上位机打开对应的输出后，才能接收到这个数据包的数据
		{
			case 0x53:	memcpy(&stcAngle,&ucRxBuffer[2],8);break;
		}
		ucRxCnt=0;//清空缓存区
	}
}


void turnAround(){
	aaaa = (float)stcAngle.Angle[2]/32768*180;
	motorControl(0, 0.7, 0, 0.7, 1); //左
	while(1){
		OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
		if((aaaa > 0) && ((float)stcAngle.Angle[2]/32768*180 < 0)){ ////////+-
			if((360 - aaaa + (float)stcAngle.Angle[2]/32768*180 ) >= 170.0){
				motorSet(0,1);
				//sp = 1;
				USART_Cmd(USART1, ENABLE);                    //使能串口1
				break;
			}
		}
		else if(((float)stcAngle.Angle[2]/32768*180 - aaaa) >= 170.0){
			motorSet(0,1);
			//sp = 1;
			USART_Cmd(USART1, ENABLE);                    //使能串口1
			break;
		}
	}
}

void turnTo(u8 data){
	while(1){
		OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
		if(flag == 1){
			flag = 0;
			motorControl(0.7, 0, 0, 0.7, 1);
			aaaa = (float)stcAngle.Angle[2]/32768*180;
			delay_ms(500);
			motorSet(data,1);
			if(data == 1){ //左
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if((aaaa > 0) && ((float)stcAngle.Angle[2]/32768*180 < 0)){ ////////+-
						if((360 - aaaa + (float)stcAngle.Angle[2]/32768*180 ) >= 70.0){
							motorSet(0,1);
							//sp = 1;
							USART_Cmd(USART1, ENABLE);                    //使能串口1
							break;
						}
					}
//					else if((aaaa < 0) && ((float)stcAngle.Angle[2]/32768*180 < 0)){ ///////--
//						if((-(180 - aaaa - (float)stcAngle.Angle[2]/32768*180 )) >= 170.0){
//							motorSet(0,1);
//							//sp = 1;
//							USART_Cmd(USART1, ENABLE);                    //使能串口1
//							break;
//						}
//					}
					else if(((float)stcAngle.Angle[2]/32768*180 - aaaa) >= 70.0){ ////////其他
						motorSet(0,1);
						//sp = 1;
						USART_Cmd(USART1, ENABLE);                    //使能串口1
						break;
					}
				}
			}
			else if(data == 2){
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if((aaaa < 0) && ((float)stcAngle.Angle[2]/32768*180 > 0)){  ////-+
						if((360 + aaaa - (float)stcAngle.Angle[2]/32768*180 ) >= 70.0){
							motorSet(0,1);
							//sp = 1;
							USART_Cmd(USART1, ENABLE);                    //使能串口1
							break;
						}
					}
//					else if((aaaa > 0) && ((float)stcAngle.Angle[2]/32768*180 < 0)){  ////+-
//						if((360 - aaaa - (float)stcAngle.Angle[2]/32768*180 ) >= 170.0){
//							motorSet(0,1);
//							//sp = 1;
//							USART_Cmd(USART1, ENABLE);                    //使能串口1
//							break;
//						}
//					}
					else if((aaaa - (float)stcAngle.Angle[2]/32768*180) >= 70.0){ //其他
						motorSet(0,1);
						//sp = 1;
						USART_Cmd(USART1, ENABLE);                    //使能串口1
						break;
					}
				}
			}

			break;
		}
	}
}

void pd(){
	if(R1 == 1){
		jilu[0] = 1;
		turnTo(1); ///////转
	}
	else if(R2 == 1){
		jilu[0] = 2;
		turnTo(2); ///////转
	}
}


void feiWuLK(){
	while(1){
		if(flag == 1){
			flag = 0;
			motorControl(0.7, 0, 0, 0.7, 1);
			delay_ms(500);
			USART_Cmd(USART1, ENABLE);
		}
	}
}

void stopCarB(){
	TIM_SetCounter(TIM4, 0);
	while(1){
		OLED_ShowVariable(64,0,2,TIM4_Getcoder());
		if(TIM4_Getcoder() >= 10000){
			LED2;
			USART_Cmd(USART1, ENABLE);                    //使能串口1
			motorSet(0, 1);
			TIM_SetCounter(TIM4, 0);
			break;
		}
	}
}

void waitMed(){
	while(1){
		if(Read_Scan() == 1){
			delay_ms(500);
			LED1Q;
			turnAround();
			break;
		}
	}
}


void logic(){
	//////项目一
	if(projects == 0){
		while(1){
			if(Read_Scan() == 0){
				LED2Q;
				delay_ms(500);
				USART_Cmd(USART1, ENABLE);
				////////////第一个掉头
				//turnAround();
				///////////走直线到十字 转弯
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if(flag == 1){
						s_Ger_Control(-0.4, 1);
						X2H;
						delay_ms(10);
						X2L;
						delay_ms(90);
						s_Ger_Control(0.4, 1);
						X2H;
						delay_ms(10);
						X2L;
						delay_ms(90);
						s_Ger_Control(0, 1);
						pd();
						break;
					}
				}
				
				ccc = 1;
				///////////等拿药   掉头回第一段路
				waitMed();
				////回药房的路
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if(flag == 1){
						if(jilu[0] == 1){
							turnTo(2); //转
						}
						else if(jilu[0] == 2){
							turnTo(1); //转
						}
						ccc = 2;
						break;
					}
				}break;
				
			}
		}
	}
	projects++;
	X1H;
	delay_ms(10);
	X1L;
	
	
	
	
	
	////////项目二
	if(projects == 1){
		while(1){
			if(Read_Scan() == 0){
				LED2Q;
				delay_ms(500);
				turnAround();
				USART_Cmd(USART1, ENABLE);
				////////////第一个掉头
				//turnAround();
				///////////走直线到第一十字
				feiWuLK();
				///////////走直线到第二十字 转弯
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if(flag == 1){
						s_Ger_Control(-0.6, 1);
						X2H;
						delay_ms(10);
						X2L;
						delay_ms(90);
						s_Ger_Control(0.6, 1);
						X2H;
						delay_ms(10);
						X2L;
						delay_ms(90);
						pd();
						break;
					}
				}
				
				ccc = 1;
				///////////等拿药   掉头回第一段路
				waitMed();
				////回药房的路
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if(flag == 1){
						if(jilu[0] == 1){
							turnTo(2); //转
						}
						else if(jilu[0] == 2){
							turnTo(1); //转
						}
						ccc = 2;
						break;
					}
				}break;
				
			}
		}
	}
	
	projects++;
	X1H;
	delay_ms(10);
	X1L;
	
	
	////////项目三
	if(projects == 2){
		while(1){
			if(Read_Scan() == 0){
				LED2Q;
				delay_ms(500);
				turnAround();
				USART_Cmd(USART1, ENABLE);
				////////////第一个掉头
				//turnAround();
				///////////走直线到第一个二十字
				feiWuLK();
				feiWuLK();
				///////////走直线到第三个十字 转弯
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if(flag == 1){
						X2H;
						s_Ger_Control(-0.6, 1);
						delay_ms(100);
						s_Ger_Control(0.6, 1);
						delay_ms(100);
						X2L;
						if(R1 == 1){
							jilu[0] = 1;
							turnTo(1); ///////转
						}
						else if(R2 == 1){
							jilu[0] = 2;
							turnTo(2); ///////转
						}
						break;
					}
				}
				///////////走直线到第四个十字 转弯
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if(flag == 1){
						s_Ger_Control(-0.6, 1);
						X2H;
						delay_ms(10);
						X2L;
						delay_ms(90);
						s_Ger_Control(0.6, 1);
						X2H;
						delay_ms(10);
						X2L;
						delay_ms(90);
						if(R1 == 1){
							jilu[1] = 1;
							turnTo(1); ///////转
						}
						else if(R2 == 1){
							jilu[1] = 2;
							turnTo(2); ///////转
						}
						break;
					}
				}
				ccc = 1;
				///////////等拿药   掉头回第一段路
				waitMed();
				////回药房的路一
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if(flag == 1){
						if(jilu[1] == 1){
							turnTo(2); //转
						}
						else if(jilu[1] == 2){
							turnTo(1); //转
						}
						ccc = 2;
						break;
					}
				}
				
				
				////回药房的路二
				while(1){
					OLED_ShowVariable(0,2,2,(float)stcAngle.Angle[2]/32768*180);
					if(flag == 1){
						if(jilu[0] == 1){
							turnTo(2); //转
						}
						else if(jilu[0] == 2){
							turnTo(1); //转
						}
						break;
					}
				}
				
				////回药房的路废十字
				feiWuLK();
				feiWuLK();
				ccc = 2;
				break;
			}
		}
	}
	
	projects++;
	X1H;
	delay_ms(10);
	X1L;
}

/*---------------------------------------------- End ------------------------------------------------*/



